package com.auto.survey.util.monitoring.intersection

import android.util.Log
import com.auto.survey.activity.monitoring.entity.MonitoringObservation
import com.auto.survey.activity.monitoring.entity.MonitoringPoint
import com.auto.survey.util.Helper
import com.auto.survey.util.monitoring.entity.CalculationResultWithDetailsTemp
import com.auto.survey.util.monitoring.entity.NonCompliantPoint
import com.auto.survey.util.monitoring.entity.RearIntersectionCalculationResult
import com.auto.survey.util.monitoring.entity.RearIntersectionVerificationResult
import org.ejml.simple.SimpleMatrix
import java.text.DecimalFormat
import kotlin.math.abs
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

/**
 * 测量算法工具类：实现后方交会（水平角）+ 边角验证（斜距）算法
 */
object RearIntersectionAlgorithm {
    val TAG = RearIntersectionAlgorithm::class.java.simpleName
    // 关键参数（适配近距场景）
    // 默认参数配置（可通过参数覆盖）
    const val DEFAULT_POSITION_THRESHOLD = 0.05      // 距离阈值：5厘米（三等控制测量标准）
    const val DEFAULT_ANGLE_THRESHOLD_SECONDS = 20.0 // 角度阈值：20秒
    const val DEFAULT_ZENITH_ANGLE_THRESHOLD_SECONDS = 30.0 // 天顶距阈值：30秒
    const val DEFAULT_MAX_ITERATIONS = 100           // 最大迭代次数
    const val DEFAULT_DAMPING_FACTOR = 0.95          // 阻尼因子（平衡收敛速度与稳定性）
    const val DEFAULT_CONVERGENCE_EPS = 1e-8        // 收敛精度：0.01毫米
    const val DEFAULT_INSTRUMENT_HEIGHT = 0.0       // 默认仪器高（米）
    const val DETERMINANT_THRESHOLD = 1e-10         // 矩阵行列式阈值


    /**
     * 基于角度和距离观测的后方交会算法（支持三维解算）
     * @param controlPoints 已知控制点列表
     * @param observations 从测站到控制点的观测数据（含水平角、天顶距和斜距）
     * @param options 算法配置选项（可选）
     * @return 计算结果（包含测站坐标、精度信息）
     */
    @JvmOverloads
    fun calculateStationPosition(
        knownPoints: List<MonitoringPoint>,
        observations: List<MonitoringObservation>,
        initialGuess: DoubleArray? = null, // 允许为空，自动计算
        options: AlgorithmOptions = AlgorithmOptions()
    ): CalculationResultWithDetailsTemp {

        // 输入验证
        require(knownPoints.size >= 3) { "至少需要3个控制点" }
        require(knownPoints.size == observations.size) { "控制点与观测值数量不匹配" }

        // 日志格式化工具
        val df = DecimalFormat("#.######")
        val angleDf = DecimalFormat("#.########")

        // 自动检测垂直角类型（如果启用）
        val effectiveVerticalAngleType = if (options.autoDetectAngleType) {
            val detectedType = detectVerticalAngleType(observations)
            Log.i(TAG, "自动检测垂直角类型为: $detectedType")
            detectedType
        } else {
            options.verticalAngleType
        }

        Log.i(TAG, "使用垂直角类型: ${effectiveVerticalAngleType}")
        // 新增：打印加权模式状态
        Log.i(TAG, "加权最小二乘法模式: ${if (options.useWeightedLeastSquares) "启用" else "禁用"}")

        // 自动计算初始值（基于第一个观测点反推）
        val defaultInitialGuess = if (knownPoints.isNotEmpty() && observations.isNotEmpty()) {
            val firstPoint = knownPoints[0]
            val firstObs = observations[0]

            // 获取转换后的垂直角（统一为高度角）
            val verticalAngle = getConvertedVerticalAngle(
                firstObs.verticalAngle,
                effectiveVerticalAngleType
            )



            // 基于观测值反推初始坐标（水平角+斜距）
            val hd = firstObs.distance * cos(verticalAngle) // 水平距
            val x = firstPoint.pointX - hd * sin(firstObs.horizontalAngle)
            val y = firstPoint.pointY - hd * cos(firstObs.horizontalAngle)
            // 问题：这里仍使用原始垂直角，未进行类型转换
            // 修正：使用转换后的垂直角计算高程
            val verticalDistance = firstObs.distance * sin(verticalAngle)
            // Z坐标计算：控制点Z + 棱镜高 - 仪器高 - 垂直距离（垂直距离向上为正，测站Z应更低）
            var z = (firstPoint.pointZ + firstPoint.mirrorHeight) - (options.instrumentHeight + verticalDistance)

            // 新增：初始Z值合理性校验（避免因模式错误导致的异常值）
            val maxZDiff = firstObs.distance * 1.5  // 允许垂直方向偏差不超过斜距的1.5倍
            if (abs(z - firstPoint.pointZ) > maxZDiff) {
                Log.w(TAG, "初始Z值偏差过大，可能是垂直角类型错误，自动修正为控制点Z附近")
                z = firstPoint.pointZ - options.instrumentHeight  //  fallback到合理值
            }

            Log.i(TAG, "verticalAngle = $verticalAngle, 约合 ${Helper.radiansToDMSString(verticalAngle)}, cos值是${cos(verticalAngle)}")
            Log.i(TAG, "水平距离 = $hd, 水平角=${firstObs.horizontalAngle},约合 ${Helper.radiansToDMSString(firstObs.horizontalAngle)}, sin值是${sin(firstObs.horizontalAngle)}")

            Log.i(TAG, "初始值计算:")
            Log.i(TAG, "  垂直角类型: ${effectiveVerticalAngleType}")
            Log.i(TAG, "  原始垂直角: ${firstObs.verticalAngle} rad (${Helper.radiansToDMSString(firstObs.verticalAngle)})")
            Log.i(TAG, "  转换后高度角: $verticalAngle rad (${Helper.radiansToDMSString(verticalAngle)})")
            Log.i(TAG, "  计算坐标: ($x, $y, $z)")

            doubleArrayOf(x, y, z)
        } else {
            doubleArrayOf(0.0, 0.0, 0.0) //  fallback
        }



        val currentInitialGuess = initialGuess ?: defaultInitialGuess


        Log.i(TAG,"开始后方交会计算...")
        Log.i(TAG,"输入参数:")
        Log.i(TAG,"  控制点数量: ${knownPoints.size}")
        Log.i(TAG,"  初始猜测坐标: (${df.format(currentInitialGuess[0])}, ${df.format(currentInitialGuess[1])}, ${df.format(currentInitialGuess[2])})")
        Log.i(TAG,"  算法选项: damping=${options.dampingFactor}, threshold=${options.convergenceThreshold}, maxIter=${options.maxIterations}")


        // 新增：坐标归一化（转换为相对坐标系，减少数值尺度差异）
        val centerX = knownPoints.map { it.pointX }.average()
        val centerY = knownPoints.map { it.pointY }.average()
        val centerZ = knownPoints.map { it.pointZ }.average()
        Log.i(TAG, "坐标归一化中心: (centerX = $centerX, centerY = $centerY, centerZ = $centerZ)")

        // 转换后的控制点坐标（相对中心坐标）
        val normalizedPoints = knownPoints.map { point ->
            point.copy(
                pointX = point.pointX - centerX,
                pointY = point.pointY - centerY,
                pointZ = point.pointZ - centerZ
            )
        }

        // 初始值同步转换
        val normalizedInitialX = currentInitialGuess[0] - centerX
        val normalizedInitialY = currentInitialGuess[1] - centerY
        val normalizedInitialZ = currentInitialGuess[2] - centerZ

        var currentX = normalizedInitialX
        var currentY = normalizedInitialY
        var currentZ = normalizedInitialZ
        Log.i(TAG, "坐标归一化后: (currentX = $currentX, currentY = $currentY, currentZ = $currentZ)")

        var iterationCount = 0
        var isConverged = false
        val n = knownPoints.size  // 控制点数量

        // 基准点（用于相对角度计算）
        val refIdx = 0
        val refPoint = normalizedPoints[refIdx]
        val refObs = observations[refIdx]
        Log.i(TAG,"选择控制点[${refPoint.pointName}]作为基准点")

        // 新增：计算观测值权重（这里基于斜距精度，可根据实际需求调整）
        // 改进权重计算：限制权重范围，避免差异过大
        // 计算所有观测点的最大距离
        val weights = if (options.useWeightedLeastSquares) {
            val maxDistance = observations.maxOfOrNull { it.distance } ?: 1.0
            val rawWeights = observations.map { obs -> 1.0 / (obs.distance * obs.distance + 0.1 * maxDistance * maxDistance) }
            val minW = rawWeights.minOrNull() ?: 1.0
            val maxW = rawWeights.maxOrNull() ?: 1.0
            val ratio = maxW / minW
            if (ratio > 1.5) {  // 若权重比超过1.5，强制缩放
                rawWeights.map { w -> minW + (w - minW) * (1.5 * minW) / (maxW - minW) }
            } else {
                rawWeights
            }.toDoubleArray()
        } else {
            DoubleArray(observations.size) { 1.0 }
        }

        // 迭代求解
        lateinit var finalA: SimpleMatrix  // 最终迭代的设计矩阵
        lateinit var finalL: SimpleMatrix  // 最终迭代的常数项向量
        lateinit var finalCorrection: SimpleMatrix  // 最终改正数

        do {
            iterationCount++
            Log.i(TAG,"=== 开始第${iterationCount}次迭代 ===")
            Log.i(TAG,"当前坐标估计值: (${df.format(currentX)}, ${df.format(currentY)}, ${df.format(currentZ)})")

            // 1. 构建EJML设计矩阵A和常数项向量L
            val A = SimpleMatrix(n * 3, 3)  // 每个观测点3个方程（水平角+水平距+天顶距）
            val L = SimpleMatrix(n * 3, 1)

            // 基准点当前坐标差（用于相对角度计算）
            val refDx = refPoint.pointX - currentX
            val refDy = refPoint.pointY - currentY
            val refDz = (refPoint.pointZ + refPoint.mirrorHeight) - (currentZ + options.instrumentHeight)
            val refHorizontalDist = sqrt(refDx * refDx + refDy * refDy)
            val refSlantDist = sqrt(refHorizontalDist * refHorizontalDist + refDz * refDz)
            val refTheoreticalHz = atan2(refDy, refDx)  // 基准点理论水平角

            Log.i(TAG,"基准点[${refPoint.pointName}]计算参数:")
            Log.i(TAG,"  坐标差: dx=${df.format(refDx)}, dy=${df.format(refDy)}, dz=${df.format(refDz)}")
            Log.i(TAG,"  水平距离: ${df.format(refHorizontalDist)}")
            Log.i(TAG,"  理论水平角: ${angleDf.format(refTheoreticalHz)} (${degrees(refTheoreticalHz)})")
            Log.i(TAG,"  基准点观测水平角: ${refObs.horizontalAngle} rad (${degrees(refObs.horizontalAngle)})")


            for (i in observations.indices) {
                val point = normalizedPoints[i]
                val obs = observations[i]
                // 2. 填充设计矩阵A（偏导数）
                val row = i * 3  // 当前点的起始行索引

                // 坐标差计算（测站到目标点）
                val dx = point.pointX - currentX
                val dy = point.pointY - currentY
                val dz = (point.pointZ + point.mirrorHeight) - (currentZ + options.instrumentHeight)
                val horizontalDist = sqrt(dx * dx + dy * dy)
                val slantDist = sqrt(horizontalDist * horizontalDist + dz * dz)

                // 理论值计算
                // 水平角相关计算（通用）
                val theoreticalHz = atan2(dy, dx)
                Log.i(TAG,"控制点[${point.pointName}]理论水平角计算:")
                Log.i(TAG,"  dx=$dx, dy=$dy → atan2(dy, dx)=${theoreticalHz} rad (${degrees(theoreticalHz)})")
                // 理论相对水平角（当前点理论角 - 基准点理论角）
                val relativeHzRaw = theoreticalHz - refTheoreticalHz
                val relativeHz = normalizeAngleResidual(theoreticalHz - refTheoreticalHz)
                Log.i(TAG,"  理论相对角: ${relativeHz} rad (${degrees(relativeHz)})")


                // 新增：观测值计算日志
                Log.i(TAG,"控制点[${point.pointName}]观测值计算:")
                Log.i(TAG,"  基准点观测水平角: ${refObs.horizontalAngle} rad (${degrees(refObs.horizontalAngle)})")
                Log.i(TAG,"  当前点观测水平角: ${obs.horizontalAngle} rad (${degrees(obs.horizontalAngle)})")


                val theoreticalVa = atan2(dz, horizontalDist)  // 理论垂直角（天顶距）
                // 残差计算（观测值 - 理论值）
                Log.i(TAG,"控制点[${point.pointName}]水平角残差计算:")
                Log.i(TAG,"  原始残差计算: obs.horizontalAngle=${obs.horizontalAngle}, relativeHz=${relativeHz}")
                val observedRelativeHzRaw = obs.horizontalAngle - refObs.horizontalAngle
                val observedRelativeHz = normalizeAngleResidual(observedRelativeHzRaw) // 新增归一化
                val hzResidual = normalizeAngleResidual(observedRelativeHz - relativeHz)  // 水平角残差
                Log.i(TAG,"  残差（原始）: ${obs.horizontalAngle - relativeHz} rad (${degrees(obs.horizontalAngle - relativeHz)})")
                Log.i(TAG,"  残差（归一化后）: ${hzResidual} rad (${degrees(hzResidual)}) → 阈值=${options.angleThresholdSeconds}秒")


                // 新增：残差超限预警
                val angleThresholdRad = Math.toRadians(options.angleThresholdSeconds / 3600.0)
                if (abs(hzResidual) > angleThresholdRad) {
                    Log.w(TAG,"⚠️ 控制点[${point.pointName}]水平角残差超限: ${degrees(hzResidual)} (阈值=${options.angleThresholdSeconds}秒)")
                }

                // 垂直角类型适配：使用if-else明确处理两种情况
                // 垂直角类型适配（核心修正）
                val (hdResidual, vaResidual, va_dx, va_dy, va_dz) = if (effectiveVerticalAngleType == VerticalAngleType.ZENITH_ANGLE) {
                    // 天顶距处理逻辑
                    val elevationAngle = Math.PI / 2 - obs.verticalAngle  // 转换为高度角
                    val hd = obs.distance * cos(elevationAngle)  // 水平距计算
                    val theoreticalVa = atan2(dz, horizontalDist)  // 理论高度角
                    val vaRes = elevationAngle - theoreticalVa  // 残差

                    // 偏导数（符号与高度角相反）
                    val v_dx = - (dx * dz) / (horizontalDist * horizontalDist * slantDist)
                    val v_dy = - (dy * dz) / (horizontalDist * horizontalDist * slantDist)
                    val v_dz = -horizontalDist / (slantDist * slantDist)

                    listOf(hd - horizontalDist, vaRes, v_dx, v_dy, v_dz)
                } else {
                    // 高度角处理逻辑
                    val elevationAngle = obs.verticalAngle
                    val hd = obs.distance * cos(elevationAngle)  // 水平距计算
                    val theoreticalVa = atan2(dz, horizontalDist)  // 理论高度角
                    val vaRes = elevationAngle - theoreticalVa  // 残差

                    // 偏导数（标准符号）
                    val v_dx = (dx * dz) / (horizontalDist * horizontalDist * slantDist)
                    val v_dy = (dy * dz) / (horizontalDist * horizontalDist * slantDist)
                    val v_dz = - horizontalDist / (slantDist * slantDist)

                    listOf(hd - horizontalDist, vaRes, v_dx, v_dy, v_dz)
                }


                // 水平距偏导数（通用，与垂直角类型无关）
                val hd_dx = -dx / horizontalDist
                val hd_dy = -dy / horizontalDist

                // 水平角偏导数（通用）
                val refDistSq = refHorizontalDist * refHorizontalDist
                val targetDistSq = horizontalDist * horizontalDist

                // 基准点极坐标参数
                val refR = refHorizontalDist
                val refTheta = refTheoreticalHz
                // 目标点极坐标参数
                val r = horizontalDist
                val theta = theoreticalHz

                // 修正后的偏导数公式
                val hz_dx = (sin(theta) * refR - sin(refTheta) * r) / (r * r * refR)
                val hz_dy = (cos(refTheta) * r - cos(theta) * refR) / (r * r * refR)


                // 新增：应用权重（每个观测点的三个方程共享同一权重）
                val weight = sqrt(weights[i])  // 权重开方（因残差平方和加权）


                // 填充设计矩阵和常数项向量（加权/非加权分支）
                if (options.useWeightedLeastSquares) {
                    // 加权模式：方程系数和残差均乘以权重
                    A.set(row, 0, hz_dx * weight)
                    A.set(row, 1, hz_dy * weight)
                    A.set(row, 2, 0.0 * weight)
                    L.set(row, 0, hzResidual * weight)

                    A.set(row + 1, 0, hd_dx * weight)
                    A.set(row + 1, 1, hd_dy * weight)
                    A.set(row + 1, 2, 0.0 * weight)
                    L.set(row + 1, 0, hdResidual * weight)

                    // 在构建A和L时，对垂直角方程额外乘以衰减系数（如0.5）
                    val verticalAngleWeightFactor = 0.5  // 降低垂直角权重
                    A.set(row + 2, 0, va_dx * weight * verticalAngleWeightFactor)
                    A.set(row + 2, 1, va_dy * weight * verticalAngleWeightFactor)
                    A.set(row + 2, 2, va_dz * weight * verticalAngleWeightFactor)
                    L.set(row + 2, 0, vaResidual * weight * verticalAngleWeightFactor)
                } else {
                    // 非加权模式：直接使用原始值
                    // 在填充设计矩阵时，水平角方程乘以1.5倍权重
                    A.set(row, 0, hz_dx *1.5)
                    A.set(row, 1, hz_dy*1.5)
                    A.set(row, 2, 0.0)
                    L.set(row, 0, hzResidual*1.5)

                    A.set(row + 1, 0, hd_dx)
                    A.set(row + 1, 1, hd_dy)
                    A.set(row + 1, 2, 0.0)
                    L.set(row + 1, 0, hdResidual)

                    A.set(row + 2, 0, va_dx)
                    A.set(row + 2, 1, va_dy)
                    A.set(row + 2, 2, va_dz)
                    L.set(row + 2, 0, vaResidual)
                }

                // 输出控制点计算详情
                Log.i(TAG,"控制点[${point.pointName}]计算详情:")
                Log.i(TAG,"  水平角残差: ${hzResidual} rad (${degrees(hzResidual)}) → ${if (abs(hzResidual) < Math.toRadians(options.angleThresholdSeconds/3600.0)) "通过" else "超限"}")
                Log.i(TAG,"  水平距残差: $hdResidual m, 垂直角残差: $vaResidual rad")
                Log.i(TAG,"  坐标差: dx=${df.format(dx)}, dy=${df.format(dy)}, dz=${df.format(dz)}")
                Log.i(TAG,"  距离: 水平=${df.format(horizontalDist)}, 斜距=${df.format(slantDist)}")
                Log.i(TAG,"  角度: 理论水平角=${angleDf.format(theoreticalHz)}, 相对水平角=${angleDf.format(relativeHz)}")
                Log.i(TAG,"  观测值: 水平角=${angleDf.format(obs.horizontalAngle)}, 垂直角=${angleDf.format(obs.verticalAngle)}, 斜距=${df.format(obs.distance)}")
                Log.i(TAG,"  残差: 水平角=${angleDf.format(hzResidual)}, 水平距=${df.format(hdResidual)}, 垂直角=${angleDf.format(vaResidual)}")
                Log.i(TAG,"  偏导数: ∂Hz/∂x=${df.format(hz_dx)}, ∂Hz/∂y=${df.format(hz_dy)}")
                Log.i(TAG,"           ∂Hd/∂x=${df.format(hd_dx)}, ∂Hd/∂y=${df.format(hd_dy)}")
                Log.i(TAG,"           ∂Va/∂x=${df.format(va_dx)}, ∂Va/∂y=${df.format(va_dy)}, ∂Va/∂z=${df.format(va_dz)}")
                if (options.useWeightedLeastSquares) {
                    Log.i(TAG,"  权重: ${weights[i]} (当前方程加权系数: $weight)")
                }


            }

            // 3. 解算法方程（带阻尼）
            Log.i(TAG,"构建法方程...")
            val AT = A.transpose()
            val ATA = AT.mult(A)
            // 添加阻尼因子（Levenberg-Marquardt）
            Log.i(TAG,"应用阻尼因子: ${options.dampingFactor}")

            // 对Z方向单独增加阻尼
//            val zDampingFactor = 2.0  // Z方向阻尼加倍
//            for (i in 0 until 3) {
//                val damping = if (i == 2) zDampingFactor else 1.0  // i=2对应Z方向
//                ATA.set(i, i, ATA.get(i, i) * (1 + options.dampingFactor * damping))
//            }

            val xYDampingFactor = 0.5  // 水平方向阻尼减半
            val zDampingFactor = 1.5   // 适当降低Z方向阻尼
            for (i in 0 until 3) {
                val damping = if (i < 2) xYDampingFactor else zDampingFactor
                ATA.set(i, i, ATA.get(i, i) * (1 + options.dampingFactor * damping))
            }

            val ATL = AT.mult(L)

            // 输出法方程系数矩阵和常数项向量的范数
            Log.i(TAG,"法方程系数矩阵ATA范数: ${ATA.normF()}")
            Log.i(TAG,"法方程常数项向量ATL范数: ${ATL.normF()}")

            val invATA = try {
                Log.i(TAG,"尝试求解法方程...")
                // 新增：检查矩阵条件数，超过阈值则增加阻尼
                val conditionNumber = ATA.conditionP2()

                val dynamicDamping = if (conditionNumber > 1e6) {
                    options.dampingFactor * (1 + Math.log10(conditionNumber / 1e6)) // 条件数越大，阻尼越强
                } else {
                    options.dampingFactor
                }

                Log.i(TAG, "法方程条件数: $conditionNumber")
                for (i in 0 until 3) {
                    ATA.set(i, i, ATA.get(i, i) * (1 + dynamicDamping))
                }

                ATA.invert()
            } catch (e: Exception) {
                Log.e(TAG,"矩阵求逆失败: ${e.message}")
                // 矩阵奇异时返回当前结果（未收敛）
                return buildFailedResult(currentX, currentY, currentZ, iterationCount, knownPoints, observations, options)
            }

            Log.i(TAG,"法方程解的条件数: ${invATA.conditionP2()}")

            // 计算改正数
            val correction = invATA.mult(ATL)

            val correctionNorm = correction.normF()  // 改正数的模

            Log.i(TAG,"计算得到坐标改正数:")
            Log.i(TAG,"  dX=${df.format(correction.get(0, 0))}, dY=${df.format(correction.get(1, 0))}, dZ=${df.format(correction.get(2, 0))}")
            Log.i(TAG,"  改正数范数: ${df.format(correctionNorm)}")

            // 4. 更新参数
            currentX += correction.get(0, 0)
            currentY += correction.get(1, 0)
            currentZ += correction.get(2, 0)

            // 在计算改正数后添加检查
            val maxCorrection = maxOf(
                abs(correction.get(0, 0)),
                abs(correction.get(1, 0)),
                abs(correction.get(2, 0))
            )
            if (maxCorrection > 10.0) { // 假设最大合理改正数为10米
                Log.e(TAG, "改正数过大，可能发散：$maxCorrection")
                return buildFailedResult(currentX, currentY, currentZ, iterationCount, knownPoints, observations, options) // 返回失败结果
            }

            // 保存最终迭代的矩阵（用于精度评定）
            finalA = A
            finalL = L
            finalCorrection = correction

            // 5. 判断收敛
            val isConvergedThisIter = correctionNorm < options.convergenceThreshold
            Log.i(TAG,"收敛判断: ${if (isConvergedThisIter) "满足" else "不满足"} (阈值=${options.convergenceThreshold})")

            if (isConvergedThisIter) {
                isConverged = true
                Log.i(TAG,"=== 迭代收敛 ===")
            } else {
                Log.i(TAG,"=== 继续迭代 ===")
            }

        } while (iterationCount < options.maxIterations && !isConverged)

        if (!isConverged) {
            Log.w(TAG,"达到最大迭代次数(${options.maxIterations})仍未收敛")
        }

        // 6. 精度评定（单位权中误差、坐标中误差）
        Log.i(TAG,"开始精度评定...")
        val (mx, my, mz, mxy) = calculatePrecision(finalA, finalL, finalCorrection, isConverged)
        Log.i(TAG,"精度评定结果:")
        Log.i(TAG,"  mx=${df.format(mx)}m, my=${df.format(my)}m, mz=${df.format(mz)}m")
        Log.i(TAG,"  mxy=${df.format(mxy)}m, mxyz=${df.format(sqrt(mx*mx + my*my + mz*mz))}m")

        // 7. 边角验证（生成verificationResults）
        Log.i(TAG,"开始边角验证...")

        val verificationResults = verifyObservations(
            knownPoints, normalizedPoints, observations,
            currentX, currentY, currentZ,
            options, refPoint, refObs,
            centerX, centerY, centerZ,
            effectiveVerticalAngleType
        )

        val passedCount = verificationResults.count { it.isPassed }
        Log.i(TAG,"边角验证结果: ${passedCount}/${verificationResults.size}个控制点通过验证")
        if (passedCount < verificationResults.size) {
            val failedPoints = verificationResults.filterNot { it.isPassed }.joinToString { it.pointName }
            Log.w(TAG,"未通过验证的控制点: $failedPoints")
        }

        // 8. 判断是否符合三级导线标准
        val isThirdOrderStandardMet = checkThirdOrderStandard(verificationResults, mxy)
        Log.i(TAG,"是否符合三级导线标准: ${if (isThirdOrderStandardMet) "是" else "否"}")

        // 计算结束后，将归一化坐标转换回原坐标系
        val finalX = currentX + centerX
        val finalY = currentY + centerY
        val finalZ = currentZ + centerZ

        // 9. 组装返回结果
        Log.i(TAG,"计算完成，返回结果:")
        Log.i(TAG,"  最终坐标: (${df.format(finalX)}, ${df.format(finalY)}, ${df.format(finalZ)})")
        Log.i(TAG,"  迭代次数: $iterationCount")
        Log.i(TAG,"  收敛状态: ${if (isConverged) "已收敛" else "未收敛"}")


        val calculationResult = RearIntersectionCalculationResult(
            stationX = finalX,
            stationY = finalY,
            stationZ = finalZ,
            mx = mx,
            my = my,
            mz = mz,
            mxy = mxy,
            mxyz = sqrt(mx * mx + my * my + mz * mz),
            iterationCount = iterationCount,
            isConverged = isConverged,
            isVerificationPassed = verificationResults.all { it.isPassed },
            isThirdOrderStandardMet = isThirdOrderStandardMet,
            meanSquareErrorOfUnitWeight = calculateUnitWeightError(finalA, finalL, finalCorrection),
            redundancy = finalA.numRows() - 3,
            totalDistance = calculateTotalDistance(
                knownPoints,
                currentX,
                currentY,
                currentZ,
                options
            ),
            meanDistance = calculateMeanDistance(
                knownPoints,
                currentX,
                currentY,
                currentZ,
                options
            ),
            maxDistanceDifference = verificationResults.maxOfOrNull { it.distanceDifference }
                ?: 0.0,
            maxAngleDifference = verificationResults.maxOfOrNull { it.angleDifference } ?: 0.0,
            maxZenithAngleDifference = verificationResults.maxOfOrNull { it.zenithAngleDifference }
                ?: 0.0
        )

        val nonCompliantPoints = verificationResults.filterNot { it.isPassed }.map {
            NonCompliantPoint(
                pointName = it.pointName
            )
        }
        val relationShip = CalculationResultWithDetailsTemp(
            calculationResult,
            verificationResults,
            nonCompliantPoints
        )

        return relationShip
    }


    // 角度归一化到[-π, π]
    private fun normalizeAngleResidual(angle: Double): Double {
        var normalized = angle % (2 * Math.PI)
        Log.i(TAG,"角度归一化过程: 输入=$angle rad (${degrees(angle)})")
        Log.i(TAG,"  取模后: $normalized rad (${degrees(normalized)})")
        if (normalized > Math.PI) {
            normalized -= 2 * Math.PI
            Log.i(TAG,"  因>π，调整后: $normalized rad (${degrees(normalized)})")
        } else if (normalized < -Math.PI) {
            normalized += 2 * Math.PI
            Log.i(TAG,"  因<-π，调整后: $normalized rad (${degrees(normalized)})")
        }
        Log.i(TAG,"  归一化结果: $normalized rad (${degrees(normalized)})")
        return normalized
    }


    /**
     * 计算精度指标（mx, my, mz, mxy）
     */
    /**
     * 计算精度指标（mx, my, mz, mxy）
     */
    private fun calculatePrecision(
        A: SimpleMatrix,
        L: SimpleMatrix,
        correction: SimpleMatrix,
        isConverged: Boolean
    ): Quadruple<Double, Double, Double, Double> {
        if (!isConverged) return Quadruple(0.0, 0.0, 0.0, 0.0)

        // 残差向量 V = A*correction - L
        val V = A.mult(correction).minus(L)
        // 自由度 = 观测数 - 未知数
        val degreesOfFreedom = A.numRows() - 3
        if (degreesOfFreedom <= 0) return Quadruple(0.0, 0.0, 0.0, 0.0)

        // 单位权中误差
        val sigma0 = sqrt(V.transpose().mult(V).get(0, 0) / degreesOfFreedom)
        // 协因数矩阵（ATA逆矩阵）
        val invATA = A.transpose().mult(A).invert()

        // X、Y、Z方向中误差
        val mx = sigma0 * sqrt(invATA.get(0, 0))
        val my = sigma0 * sqrt(invATA.get(1, 1))
        val mz = sigma0 * sqrt(invATA.get(2, 2))
        // 平面点位中误差
        val mxy = sqrt(mx * mx + my * my)

        return Quadruple(mx, my, mz, mxy)
    }

    /**
     * 验证观测值与理论值的一致性
     *
     * @param knownPoints 原始控制点列表（用于获取点ID和名称）
     * @param normalizedPoints 归一化后的控制点列表
     * @param observations 观测数据列表
     * @param stationX 归一化后的测站X坐标
     * @param stationY 归一化后的测站Y坐标
     * @param stationZ 归一化后的测站Z坐标
     * @param options 算法选项
     * @param refPoint 归一化后的基准点
     * @param refObs 基准点的观测数据
     * @param centerX 归一化中心X坐标（保留参数，当前用于日志记录）
     * @param centerY 归一化中心Y坐标（保留参数，当前用于日志记录）
     * @param centerZ 归一化中心Z坐标（保留参数，当前用于日志记录）
     * @param verticalAngleType 垂直角类型（天顶距或高度角）
     * @return 验证结果列表
     */
    private fun verifyObservations(
        knownPoints: List<MonitoringPoint>,
        normalizedPoints: List<MonitoringPoint>,
        observations: List<MonitoringObservation>,
        stationX: Double,
        stationY: Double,
        stationZ: Double,
        options: AlgorithmOptions,
        refPoint: MonitoringPoint,
        refObs: MonitoringObservation,
        centerX: Double,
        centerY: Double,
        centerZ: Double,
        verticalAngleType: VerticalAngleType
    ): List<RearIntersectionVerificationResult> {
        // 基准点垂直角转换（统一转换为高度角）
        val refVerticalAngle = getConvertedVerticalAngle(refObs.verticalAngle, verticalAngleType)

        // 基准点计算
        val refDx = refPoint.pointX - stationX
        val refDy = refPoint.pointY - stationY
        val refHorizontalDist = sqrt(refDx * refDx + refDy * refDy)
        val refTheoreticalHz = atan2(refDy, refDx)

        // 记录归一化中心信息（用于调试）
        Log.i(TAG, "归一化中心: ($centerX, $centerY, $centerZ)")
        Log.i(TAG, "基准点[${knownPoints[0].pointName}]计算参数:")
        Log.i(TAG, "  垂直角类型: $verticalAngleType")
        Log.i(TAG, "  原始垂直角: ${refObs.verticalAngle} rad (${degrees(refObs.verticalAngle)})")
        Log.i(TAG, "  转换后高度角: ${refVerticalAngle} rad (${degrees(refVerticalAngle)})")
        Log.i(TAG, "  坐标差: dx=${refDx}, dy=${refDy}, 水平距离=${refHorizontalDist}")
        Log.i(TAG, "  理论水平角: ${refTheoreticalHz} rad (${degrees(refTheoreticalHz)})")

        return knownPoints.zip(observations).mapIndexed { index, (point, obs) ->
            val normalizedPoint = normalizedPoints[index]

            // 坐标差计算（测站到目标点）
            val dx = normalizedPoint.pointX - stationX
            val dy = normalizedPoint.pointY - stationY
//            val dz = (point.pointZ + point.mirrorHeight) - (stationZ + options.instrumentHeight)

            // 修正：将归一化的stationZ转换为实际坐标后再计算dz
            val actualStationZ = stationZ + centerZ  // 反归一化（centerZ是归一化中心Z）
            val dz = (point.pointZ + point.mirrorHeight) - (actualStationZ + options.instrumentHeight)

            val horizontalDist = sqrt(dx * dx + dy * dy)
            val slantDist = sqrt(horizontalDist * horizontalDist + dz * dz)



            // 当前点垂直角转换（统一转换为高度角）
            val verticalAngle = getConvertedVerticalAngle(obs.verticalAngle, verticalAngleType)

            // 理论值计算
            val theoreticalHz = atan2(dy, dx)  // 理论水平角
            val relativeHz = normalizeAngleResidual(theoreticalHz - refTheoreticalHz)  // 相对水平角

            // 观测值计算（考虑基准点）
            val observedRelativeHz = normalizeAngleResidual(obs.horizontalAngle - refObs.horizontalAngle)

            // 残差计算
//            val angleDiff = normalizeAngleResidual(observedRelativeHz - relativeHz)  // 水平角残差

            var angleDiff = normalizeAngleResidual(observedRelativeHz - relativeHz)
            if (abs(angleDiff) > Math.PI / 2) {
                angleDiff += if (angleDiff > 0) -Math.PI else Math.PI  // 转换到最小残差
            }

            // 新增：验证阶段水平角残差详细日志
            Log.i(TAG,"验证阶段-控制点[${point.pointName}]水平角残差:")
            Log.i(TAG,"  理论相对角: $relativeHz rad (${degrees(relativeHz)})")
            Log.i(TAG,"  观测相对角: $observedRelativeHz rad (${degrees(observedRelativeHz)})")
            Log.i(TAG,"  最终残差: $angleDiff rad (${degrees(angleDiff)})，阈值=${Math.toRadians(options.angleThresholdSeconds/3600.0)} rad")


            // 水平距离计算（使用转换后的垂直角）
            val observedHd = obs.distance * cos(verticalAngle)  // 注意：使用转换后的高度角
            val distDiff = observedHd - horizontalDist  // 水平距残差

            // 相对距离误差
            val relativeDistDiff = if (observedHd != 0.0) distDiff / observedHd else 0.0

            // 理论垂直角（高度角，与水平线夹角）
            val theoreticalVa = atan2(dz, horizontalDist)  // 实际是理论高度角（与水平线夹角）
            val vaDiff = verticalAngle - theoreticalVa  // 垂直角残差（高度角差值）

            // 判断是否通过验证
            val angleThresholdRad = Math.toRadians(options.angleThresholdSeconds / 3600.0)
            val distanceThreshold = options.positionThreshold
            val vaThresholdRad = Math.toRadians(options.zenithAngleThresholdSeconds / 3600.0)

            val isAnglePassed = abs(angleDiff) < angleThresholdRad
            val isDistancePassed = abs(distDiff) < distanceThreshold
            val isVaPassed = abs(vaDiff) < vaThresholdRad

            // 记录详细日志（前3个点或验证失败的点）
            if (index < 3 || !isAnglePassed || !isDistancePassed || !isVaPassed) {
                Log.i(TAG, "控制点[${point.pointName}]计算详情:")
                Log.i(TAG, "  原始垂直角: ${obs.verticalAngle} rad (${degrees(obs.verticalAngle)})")
                Log.i(TAG, "  转换后高度角: ${verticalAngle} rad (${degrees(verticalAngle)})")
                Log.i(TAG, "  理论垂直角: ${theoreticalVa} rad (${degrees(theoreticalVa)})")
                Log.i(TAG, "  垂直角残差: ${vaDiff} rad (${degrees(vaDiff)})")
                Log.i(TAG, "  垂直角阈值: ${vaThresholdRad} rad (${options.zenithAngleThresholdSeconds}秒)")
                Log.i(TAG, "  垂直角验证: ${if (isVaPassed) "通过" else "失败"}")

                Log.i(TAG, "  坐标差: dx=$dx, dy=$dy, dz=$dz")
                Log.i(TAG, "  距离: 水平=$horizontalDist m, 斜距=$slantDist m")
                Log.i(TAG, "  观测斜距: ${obs.distance} m")
                Log.i(TAG, "  观测水平距: ${observedHd} m")
                Log.i(TAG, "  水平距残差: ${distDiff} m")
                Log.i(TAG, "  水平距阈值: ${distanceThreshold} m")
                Log.i(TAG, "  水平距验证: ${if (isDistancePassed) "通过" else "失败"}")

                Log.i(TAG, "  理论水平角: ${theoreticalHz} rad (${degrees(theoreticalHz)})")
                Log.i(TAG, "  观测水平角: ${obs.horizontalAngle} rad (${degrees(obs.horizontalAngle)})")
                Log.i(TAG, "  相对水平角: 理论=${relativeHz} rad (${degrees(relativeHz)}), 观测=${observedRelativeHz} rad (${degrees(observedRelativeHz)})")
                Log.i(TAG, "  水平角残差: ${angleDiff} rad (${degrees(angleDiff)})")
                Log.i(TAG, "  水平角阈值: ${angleThresholdRad} rad (${options.angleThresholdSeconds}秒)")
                Log.i(TAG, "  水平角验证: ${if (isAnglePassed) "通过" else "失败"}")

                Log.i(TAG, "  控制点[${point.pointName}]验证汇总:")
                Log.i(TAG, "    垂直角残差: ${degrees(vaDiff)}（阈值: ${options.zenithAngleThresholdSeconds}秒）→ ${if (isVaPassed) "通过" else "失败"}")
                Log.i(TAG, "    水平距残差: ${String.format("%.6f", distDiff)} m（阈值: ${distanceThreshold} m）→ ${if (isDistancePassed) "通过" else "失败"}")
                Log.i(TAG, "    水平角残差: ${degrees(angleDiff)}（阈值: ${options.angleThresholdSeconds}秒）→ ${if (isAnglePassed) "通过" else "失败"}")
                Log.i(TAG, "    整体验证结果: ${if (isAnglePassed && isDistancePassed && isVaPassed) "通过" else "失败"}")
            }

            RearIntersectionVerificationResult(
                pointId = point.id,
                pointName = point.pointName,
                distanceDifference = distDiff,
                angleDifference = angleDiff,
                relativeDistanceDifference = relativeDistDiff,
                zenithAngleDifference = vaDiff,
                isDistancePassed = isDistancePassed,
                isAnglePassed = isAnglePassed,
                isZenithAnglePassed = isVaPassed,
                isPassed = isAnglePassed && isDistancePassed && isVaPassed
            )
        }
    }


    /**
     * 检查是否符合三级导线标准
     */
    private fun checkThirdOrderStandard(
        verificationResults: List<RearIntersectionVerificationResult>,
        mxy: Double
    ): Boolean {
        // 三级导线标准：平面点位中误差≤±10mm，角度闭合差≤±12″√n，边长相对中误差≤1/15000
        val n = verificationResults.size  // 边数

        // 检查平面点位中误差
        val positionAccuracyPassed = mxy <= 0.010  // ≤10mm

        // 检查角度闭合差（简化：检查最大角度差值）
        val maxAngleDiff = verificationResults.maxOfOrNull { it.angleDifference } ?: 0.0
        val angleAccuracyPassed = abs(maxAngleDiff) <= Math.toRadians(12.0 / 3600.0 * sqrt(n.toDouble()))

        // 检查边长相对中误差（简化：检查最大相对距离误差）
        val maxRelativeDistDiff = verificationResults.maxOfOrNull { it.relativeDistanceDifference } ?: 0.0
        val distanceAccuracyPassed = maxRelativeDistDiff <= 1.0 / 15000.0

        return positionAccuracyPassed && angleAccuracyPassed && distanceAccuracyPassed
    }


    /**
     * 计算单位权中误差
     */
    private fun calculateUnitWeightError(
        A: SimpleMatrix,
        L: SimpleMatrix,
        correction: SimpleMatrix
    ): Double {
        val V = A.mult(correction).minus(L)
        val degreesOfFreedom = A.numRows() - 3
        return if (degreesOfFreedom > 0) {
            sqrt(V.transpose().mult(V).get(0, 0) / degreesOfFreedom)
        } else {
            0.0
        }
    }

    /**
     * 计算总距离
     */
    private fun calculateTotalDistance(
        knownPoints: List<MonitoringPoint>,
        stationX: Double,
        stationY: Double,
        stationZ: Double,
        options: AlgorithmOptions
    ): Double {
        return knownPoints.sumOf { point ->
            val dx = point.pointX - stationX
            val dy = point.pointY - stationY
            val dz = (point.pointZ + options.instrumentHeight) - stationZ
            sqrt(dx * dx + dy * dy + dz * dz)
        }
    }

    /**
     * 计算平均距离
     */
    private fun calculateMeanDistance(
        knownPoints: List<MonitoringPoint>,
        stationX: Double,
        stationY: Double,
        stationZ: Double,
        options: AlgorithmOptions
    ): Double {
        val total = calculateTotalDistance(knownPoints, stationX, stationY, stationZ, options)
        return if (knownPoints.isNotEmpty()) total / knownPoints.size else 0.0
    }

    /**
     * 构建未收敛时的结果
     */
    /**
     * 构建未收敛时的结果
     */
    private fun buildFailedResult(
        x: Double,
        y: Double,
        z: Double,
        iterationCount: Int,
        knownPoints: List<MonitoringPoint>,
        observations: List<MonitoringObservation>,
        options: AlgorithmOptions
    ): CalculationResultWithDetailsTemp {
        val verificationResults = knownPoints.zip(observations).map { (point, obs) ->
            RearIntersectionVerificationResult(
                pointId = point.id,
                pointName = point.pointName,
                distanceDifference = 0.0,
                angleDifference = 0.0,
                relativeDistanceDifference = 0.0,
                zenithAngleDifference = 0.0,
                isDistancePassed = false,
                isAnglePassed = false,
                isZenithAnglePassed = false,
                isPassed = false
            )
        }

        val calculationResult = RearIntersectionCalculationResult(
            stationX = x,
            stationY = y,
            stationZ = z,
            mx = 0.0,
            my = 0.0,
            mz = 0.0,
            mxy = 0.0,
            mxyz = 0.0,
            iterationCount = iterationCount,
            isConverged = false,
            isVerificationPassed = false,
            isThirdOrderStandardMet = false,
            meanSquareErrorOfUnitWeight = 0.0,
            redundancy = 0,
            totalDistance = 0.0,
            meanDistance = 0.0,
            maxDistanceDifference = 0.0,
            maxAngleDifference = 0.0,
            maxZenithAngleDifference = 0.0
        )

        val nonCompliantPoints = verificationResults.filterNot { it.isPassed }.map {
            NonCompliantPoint(
                pointName = it.pointName
            )
        }
        val relationShip = CalculationResultWithDetailsTemp(
            calculationResult,
            verificationResults,
            nonCompliantPoints
        )

        return relationShip
    }

    // 角度转度分秒字符串（辅助日志）
    private fun degrees(radians: Double): String {
        val deg = Math.toDegrees(radians)
        val d = deg.toInt()
        val m = ((deg - d) * 60).toInt()
        val s = ((deg - d - m / 60.0) * 3600)
        return "${d}°${m}'${s.toString().substring(0, minOf(5, s.toString().length - 1))}\""
    }

    // 获取经过类型转换的垂直角（统一转换为高度角）
    private fun getConvertedVerticalAngle(rawAngle: Double, type: VerticalAngleType): Double {
        return when (type) {
            VerticalAngleType.ZENITH_ANGLE -> {
                // 天顶距范围应在0~π，超出则修正
                val clamped = rawAngle.coerceIn(0.0, Math.PI)
                Math.PI/2 - clamped // 转高度角
            }
            VerticalAngleType.VERTICAL_ANGLE -> rawAngle.coerceIn(-Math.PI/2, Math.PI/2)
        }
    }

    // 自动检测垂直角类型
    /**
     * 自动检测垂直角类型（天顶距或高度角）
     *
     * 算法逻辑：
     * 1. 统计在天顶距范围(0°~180°)内的角度数量
     * 2. 统计在高度角范围(-90°~90°)且明显非天顶距的角度数量
     * 3. 优先考虑角度分布的集中区域
     * 4. 特别处理接近90°天顶距的情况
     */
    private fun detectVerticalAngleType(observations: List<MonitoringObservation>): VerticalAngleType {
        // 定义角度范围（弧度）
        val zenithRange = 0.0..Math.PI  // 天顶距范围：0°~180°
        val verticalRange = -Math.PI/2..Math.PI/2  // 高度角范围：-90°~90°

        // 定义模糊区域（接近90°天顶距的区域）
        val fuzzyThreshold = Math.toRadians(80.0)  // 80°~100°天顶距作为模糊区域
        val zenithFuzzyRange = (Math.PI/2 - fuzzyThreshold)..(Math.PI/2 + fuzzyThreshold)

        // 统计各种角度出现的次数
        var zenithCount = 0
        var verticalCount = 0
        var fuzzyCount = 0

        observations.forEach { obs ->
            val angle = obs.verticalAngle

            if (zenithRange.contains(angle)) {
                if (zenithFuzzyRange.contains(angle)) {
                    fuzzyCount++  // 模糊区域单独计数
                } else {
                    zenithCount++  // 明确的天顶距
                }
            }

            if (verticalRange.contains(angle) && !zenithFuzzyRange.contains(angle)) {
                verticalCount++  // 明确的高度角（排除模糊区域）
            }
        }

        // 计算明确的角度类型占比
        val totalCertain = zenithCount + verticalCount
        val zenithRatio = if (totalCertain > 0) zenithCount.toDouble() / totalCertain else 0.0
        val verticalRatio = if (totalCertain > 0) verticalCount.toDouble() / totalCertain else 0.0

        // 日志记录（方便调试）
        Log.i(TAG, "垂直角类型检测统计:")
        Log.i(TAG, "  天顶距范围(明确): $zenithCount")
        Log.i(TAG, "  高度角范围(明确): $verticalCount")
        Log.i(TAG, "  模糊区域(接近90°天顶距): $fuzzyCount")
        Log.i(TAG, "  天顶距占比: ${zenithRatio.percent()}")
        Log.i(TAG, "  高度角占比: ${verticalRatio.percent()}")

        // 判断逻辑：
        // 1. 如果明确的天顶距占比显著更高，判断为天顶距
        // 2. 如果明确的高度角占比显著更高，判断为高度角
        // 3. 如果无法明确区分或模糊区域占比高，默认返回天顶距（更常见的全站仪设置）
        return if (zenithRatio > 0.7) {
            VerticalAngleType.ZENITH_ANGLE
        } else if (verticalRatio > 0.7) {
            VerticalAngleType.VERTICAL_ANGLE
        } else {
            Log.i(TAG, "垂直角类型检测结果不明确，默认返回天顶距")
            VerticalAngleType.ZENITH_ANGLE
        }
    }

    // 辅助函数：将小数转换为百分比字符串
    private fun Double.percent(): String = "${(this * 100).format(1)}%"

    // 辅助函数：格式化小数
    private fun Double.format(digits: Int) = "%.${digits}f".format(this)



}